COMMENT ⊗   VALID 00005 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	\!head2(MOTION EXECUTION AND CONTROL)
C00007 00003	\!head3(Motion Preparation and Initiation)
C00014 00004	\!head3(Real-time Device Control)
C00022 00005	\!head3(Manipulator Kinematic and Dynamic Evaluation)
C00027 ENDMK
C⊗;
\!head2(MOTION EXECUTION AND CONTROL);


\F0\JThe motion execution and control programs are used to translate
commands from the runtime interpreters into device motions.  In order
to accomplish this task, this code includes control algorithms to
close the motor servo loops within the computer as well as analytical
programs to deal with the kinematics and dynamics of the mechanisms
known to AL.  These routines occupy approximately 6.5k words of
memory in the PDP 11/45.

In this section, we will be using the term "joint" to describe any
controllable element of a device.  For example, the manipulator's six
electrically driven links, its hand, and the vise will each be
refered to as a device "joint".  In addition, the data and the
computer code necessary to drive each joint will be refered to as a
"joint servo" or simply a "servo".

The subroutines that will be described in this section can be grouped
according to their functions as follows:\.

\j1. \{Motion Preparation and Initiation:\}=8;=3; These are the top
level motion subroutines that accept commands from the runtime
interpreters.  Planned trajectories are always checked for their
validity and any necessary modifications are accomplished by these
routines.  Once trajectory preparation is completed, a separate servo
job is initiated for each joint that is to be run ( see below ).  In
addition, the interpreters can use these routines to determine the
current configuration of any device.\.

\j2. \{Real Time Device Control:\}=8;=3; These programs are used to
servo joints along the desired polynomial trajectories.  Each joint
servo runs independently and is executed at regular intervals.
Typically, a joint will be serviced every 1/60 of a second.
Position, velocity, and integral feedback are employed whenever
possible to obtain a smooth and stiff response.\.

\j3. \{Manipulator Kinematic and Dynamic Evaluation:\}=8;=3; This
code includes the following algorithms: arm solution programs that
convert from spatial transforms to manipulator joint angles and back,
predictor programs for estimating the dynamic coefficients of the arm
for any given configuration, and a routine for resolving reaction
forces and moments applied at the manipulator hand into equivalent
joint reaction torques.\.


\JThe data area associated with these subroutines contains the
calibration information required for each joint along with other
information necessary for the operation of the servos.  The specifics
of the data area and a description of each of the subroutines
in this module follows.\.


\,
\!head3(Motion Preparation and Initiation);


\F0\JThe following subroutines serve as the interface between the
runtime interpreters and the control code.  Each of these routines
requires as argument a "device block" which is used to store
pointers to any joints that are referenced while the subroutines are
executing.  After execution, the device block contains error
information relating to the specific joints.  The or'ed result of all
of the joint error bits and additional error bits set by the called
subroutine are returned along with the information saved in the
device block for use by the calling interpreter.  The other argument
required by some of these routines is a "coefficient list".  This
list is the motion table emitted by the
trajectory calculator and pointed to by the pseudo-code MOVE.
It contains all of the information necessary to specify device
motions.  The format of this data structure is given elsewhere in
this document.  A description of each of these routines follows.\.


Subroutine INTARM (DEVICE BLOCK)

\JInitializes the arm servo routines  by performing the following operations:
sets up DAC and PANIC BUTTON interrupt vectors, verifies that the 
reference power supply is within range, averages zero velocity tachometer
readings for all joints with tachometers and stores the value as a zero
offset, and updates the current joint angle, joint inertia  and gravity
loading variables in the servo device blocks.\.


Subroutine DRIVE (DEVICE BLOCK, COEFFICIENT LIST)

\JThis subroutine is intended for use primarily as a diagnostic tool.  It
initiates a differential change in joint angle for a single servo.  The
trajectory is only corrected for start position and no transform
data is utilized to modify planned polynomials.\.


Subroutine CENTER (DEVICE BLOCK,COEFFICIENT LIST)

\JCloses the fingers of the specified manipulator hand and centers the arm
over any grasped object.  This routine uses the finger touch
sensors to determine when contact has been made.  Special ADC condition monitors
are employed to trigger the various stages of this motion.\.


Subroutine WHERE (DEVICE BLOCK, COEFFICIENT LIST)

\JReads the current joint angles for any requested joints and returns the
data in the coefficient list.  As a side effect, the joint angle, joint
inertia and gravity loading of each requested joint are updated in the
joint data blocks.  This routine should be called just prior to     
executing any motion where manual re-positioning of the device to be 
driven was possible.\.


Subroutine MOVE (DEVICE BLOCK, COEFFICIENT LIST)

\JMoves a collection of joints along polynomial trajectories.  The
joints need not include all of the links of a single arm and can
include joints from different physical devices.  The trajectories can
consist of an arbitrary number of segments, however, each segment must
include a polynomial for each of the specified joints together with
gravity and inertia data for the segment end point.  If a planned
trajectory is found to be invalid due to a change in the start or end
points of any of its segments it is corrected by this routine before
the motion is initiated.  Trajectory modification is accomplished by
adding fifth order polynomials to the planned polynomials.  If two
via points have one or more free points between them, all of the
segments between the via points are treated as one single segment and
the fifth order correction polynomial is applied across this single
long segment.\.


\JThe support subroutines required by the routines presented above are:
ATTSRV, RELSRV, MAK5TH, SETSET, TOUCH, CLRTCH, SMPTCH, STPTCH,
TCHLV7, SETTH, SETREF, STHLV7, SRFLV7, and RUNSRV.  For a description
of these routines, please refer to the original source code, ARM.PAL\.


\,
\!head3(Real-time Device Control);


\F0\JEach of the joints known to the system has a block of data
associated with it that contains all of the information required to
servo that joint.  Some typical information stored in the servo data
blocks is the calibration information to convert from ADC
readings to joint angle, and the current joint angle and joint
velocity.  The first two words of each servo data block are used as
bit registers to indicate the current status of the joint and its
current or forthcoming mode of operation.  The status bits are
defined as follows:\.
	
\F3\;
        FIRST  	first pass thru servo
        MOVING  device in motion, i.e. its velocity is not known to be zero
        RUN  	run servo, set to zero to stop servoing
        FINAL	in final stage of motion, just nulling errors
        STROUT 	joint started outside of permitted operating range
        NONEX	joint is down, inoperable
        ADERR	catastrophic ADC error has occurred
        PANCBT	panic button has been pushed
        EXJTFC	excessive joint force error
        TMEOUT  function took too long to execute
        NNUL	do not null position error at the end of the trajectory
        WOBBLE  wobble outer joints at end of trajectory
        DEPTPT 	motion has a departure point
\F0\;

\JWhile the joints are running, each joint is controlled by the same
set of subroutines.  For the most part, each of these subroutines
requires as its only argument a pointer to the servo data block for
the joint that is to be serviced.  Currently, it takes approximately
1.5 milliseconds to servo a single joint.  This estimate includes the
time required for the kernel to read the ADC and switch jobs.  A
description of these subroutines follows in the approximate order in
which they are executed:\.


Subroutine SERVO

\JBased upon the information contained in the servo data block, this
routine decides what needs to be done during this servicing interval.
It is possible for the joint to be in a start wait state, a running
condition, or the motion might be completed and only the nulling of
final position and velocity errors needs to be done.  Also, the panic
button may have been hit since the last servicing period or some
other joint may have signaled that a catastrophic error has occurred
in which case this routine initiates the stopping sequence.\.


Subroutine ANGLES

\JTakes the ADC readings stored in the joint data block by the kernel
and converts them to joint angle and velocity.\.


Subroutine FEEDBACK

\JComputes the output motor torque based upon the joint position,
velocity, and integral error feedback and the effects of gravity
loading, friction, and inertial acceleration.  The implemented
response equation is as follows:\.

	Motor Torque = CII*THDDP + CI - KE*(TH-THP) - KV*CII*(THD-THDP)
		      - (KI/CII)*Integral(TH-THP) + V0
   where
\F3\;
		CII  = joint inertia
		CI   = joint gravity loading
		TH   = actual joint angle
	   	THP  = predicted joint angle
		THD  = actual joint angular velocity
		THDP = predicted joint angular velocity
		THDDP= predicted joint angular acceleration
		V0   = friction compensation with the same sign as velocity
		KE,KI,KV = constant feedback coefficients
\F0\;

\JThe integral of position error is developed by summing the position error each
time the servo is serviced.  For joints that suffer from excessive static
friction, a dither term is also added to the equation above.\.


Subroutine SCHED

\JRe-schedules the servo to run after a minimum elapsed time has passed.
Knowing exactly when the servo is to run again allows us to evaluate
all of the time dependent variables for the next servicing interval
in advance, thus minimizing the delay between reading the ADC and
computing the feedback error response.\.


Subroutine EVAL

\JThe joint position, velocity, acceleration, inertia, and gravity
loading are evaluated and stored for the next servicing period.  The
new position is computed as the sum of the value of the trajectory
polynomial and the integral of a small correction velocity term.
This velocity term is used to perform  differential motions.
At present, differential motions are only employed in
connection with the CENTER function, but later we hope to utilize this
type of motion in feedback loops involving external sensory devices, e.g.
touch, vision.  If at any time, the desired joint position is out of
the permitted range of movement, a special function is employed to
modify the set point.  The velocity and acceleration are computed by
forward differencing the joint position.  The joint inertia and
gravity loading are computed by linearly interpolating between values
given for the end points of trajectory segments.\.


Subroutine WIPER

\JSwitches ADC channels if the ADC reading is out of range and the
joint has multiple pot wipers.\.


Subroutines MOTDRV,MOTSTP,BRKREL

\JUsed to generate the digital-to-analog converter signals to the 
joint motors and operate the joint brakes.\.


\,
\!head3(Manipulator Kinematic and Dynamic Evaluation);


\F0\JThe following subroutines deal specifically with the kinematics
and dynamics of the Stanford Scheinman Arm.  In the argument lists
of the subroutines discussed below, ANGLES refers to a array containing
the six joint angles necessary to uniquely specify an arm's 
configuration, TRANSFORM is a 4 by 4 matrix which defines a point 
in space and an orientation, and MECHANISM indicates the arm under
consideration.\.


Subroutine SOLVE ( TRANSFORM, ANGLES, MECHANISM, ERROR )

\JGiven a transform, the equivalent joint angles are determined.  If
any joint angle is outside the permitted range of movement, the
angle returned is the average of the old joint angle and the joint
stop limit.  In the determination of joint angle 4, if the required
change in angle is greater than 90 degrees and less than 180, the
hand is flipped over and the joint angle change becomes THETA =
THETA-90.  The number of joints out of range is returned in the ERROR
word.  The nominal execution time is approximately 4450 microseconds.\.


Subroutine UPDATE ( TRANSFORM, ANGLES, MECHANISM )

\JGiven the joint angles, the resulting hand position and orientation are
determined and stored in a transform array.  The nominal execution time
is approximately 5700 microseconds.\.


Subroutine DTERMS ( ANGLES, DYNAMIC COEF., MECHANISM)

\JComputes the diagonal terms of the acceleration coefficients
matrix and the gravity matrix for a given arm configuration.  The
implemented equations were derived in "Robot  Arm Dynamics and
Control, JPL Memorandum 33-669", by A.K. Bejczy.  This routine
executes in approximately 1 millisecond.\.


Subroutine HNDWT ( ANGLES, OBJECT, MECHANISM )

\JModifies the arm dynamic coefficients to take into consideration
the inertia and gravity loading of an object held between the finger
tips of the manipulator's hand.  The mass of the object and its
radii of gyration must be specified.  The execution time required 
by this routine is approximately 260 microseconds.\.


Subroutine FORCE ( ANGLES, FORCE ARRAY, REACTION ARRAY, MECH. )

\JComputes the reaction joint torques for a given force and moment
applied at the hand and specified in hand coordinates.  The reaction
torques are computed by successively transforming the given force and moment
into joint coordinate systems.  The transformations are accomplished by 
multiplying the force and moment by the "A" matrix of each of the arm links.
After transformation, the joint reactions are just the Z-components of the 
transformed moment in each of the joint coordinate reference frames.\.


\JStandard  routines for computing square roots, sine, cosines,
tangents, and the inverses of these three trigonometric functions have
been written. The subroutine names are:  SQRT, SNCOS, ASIN, ACOS,
ATAN, ATAN2.\.